#include "starrobot_base.h"

starrobotBase::starrobotBase():
    linear_velocity_x_(0),pub_encoder_odom_tf_(false),encoder_angle_z_(0.0),
    linear_velocity_y_(0),angular_velocity_z_(0),encoder_data_y_(0.0),
    last_vel_time_(0),vel_dt_(0),x_pos_(0),encoder_data_x_(0.0),
    y_pos_(0),heading_(0),is_sub_node_(true),is_timer(true),resetOdom(true),
    linear_scale_correction_(1.0),angular_scale_wheel_(1.0),baseType("d2"),
    motor_encoder1_old(0.0),motor_encoder2_old(0.0),lr_wheels_distance_(1.0),
    motor_encoder3_old(0.0),motor_encoder4_old(0.0),fr_wheels_distance_(1.0),
    motor_encoder1_inc(0.0),motor_encoder2_inc(0.0),Coefficient(1.0),
    motor_encoder3_inc(0.0),motor_encoder4_inc(0.0)
{
	ros::NodeHandle nh_get("~");
	nh_get.getParam("/starrobot_base_sub_mane", sub_name_);
	nh_get.getParam("/starrobot_base_pub_mane", pub_name_);
	nh_get.getParam("/pub_encoder_odom_tf", pub_encoder_odom_tf_);

	nh_get.getParam("/starrobot_base_tf_frame_id_name", tf_frame_id_name_);
	nh_get.getParam("/starrobot_base_tf_child_frame_id_name", tf_child_frame_id_name_);
	nh_get.getParam("/starrobot_base_odom_frame_id_name", odom_frame_id_name_);
	nh_get.getParam("/starrobot_base_odom_child_frame_id_name", odom_child_frame_id_name_);
    nh_get.getParam("/starrobot_base", baseType);
    nh_get.getParam("/lr_wheels_distance", lr_wheels_distance_);
    nh_get.getParam("/fr_wheels_distance", fr_wheels_distance_);
	odom_publisher_ = nh_.advertise<nav_msgs::Odometry>(pub_name_, 50);
	velocity_subscriber_ = nh_.subscribe(sub_name_, 50, &starrobotBase::velCallback, this);

	nh_get.getParam("linear_scale_correction", linear_scale_correction_);
	nh_get.getParam("angular_scale_wheel", angular_scale_wheel_);
	if(pub_encoder_odom_tf_)
	{
		ROS_INFO("publisher encoder odom tf ");
	}
	if(!pub_encoder_odom_tf_)
	{
		ROS_INFO("No publisher encoder odom tf ");
	}
	ROS_INFO("linear_scale_correction:%f",linear_scale_correction_);
	ROS_INFO("angular_scale_wheel:%f",angular_scale_wheel_);
}
void starrobotBase::velCallback(const starrobot_msgs::Velocities& vel)
{   
	ros::NodeHandle nh_get("~");
    linear_velocity_x_ = vel.linear_x ;
    linear_velocity_y_ = vel.linear_y ;
    angular_velocity_z_ = vel.angular_z ;
    if(resetOdom)
    {
       motor_encoder1_old = vel.encoder_motor1;
       motor_encoder2_old = vel.encoder_motor2;
       motor_encoder3_old = vel.encoder_motor3;
       motor_encoder4_old = vel.encoder_motor4;
       resetOdom = false;
    }
    motor_encoder1_inc = vel.encoder_motor1 - motor_encoder1_old;
    motor_encoder2_inc = vel.encoder_motor2 - motor_encoder2_old;
    motor_encoder3_inc = vel.encoder_motor3 - motor_encoder3_old;
    motor_encoder4_inc = vel.encoder_motor4 - motor_encoder4_old;
    //ROS_INFO("E_inc1:%.3f E_inc2:%.3f",motor_encoder1_inc,motor_encoder2_inc);

	ros::Time current_time = ros::Time::now();
	if(is_timer)
	{
		last_vel_time_ = current_time;
		is_timer = false;
	}
	if(baseType == "d2" || baseType == "t2")
	{   
		Coefficient = lr_wheels_distance_ / 2.0;
		encoder_data_x_  = (motor_encoder1_inc + motor_encoder2_inc)/2.0;
		encoder_data_y_  = 0.0;
		encoder_angle_z_ = ((motor_encoder2_inc - motor_encoder1_inc)/2.0)/Coefficient;
		encoder_data_x_  = encoder_data_x_ * linear_scale_correction_;
		encoder_angle_z_ = encoder_angle_z_* angular_scale_wheel_;		
		//ROS_INFO("E_inc1:%.3f E_inc2:%.3f",motor_encoder1_inc,motor_encoder2_inc);
	}
	else if(baseType == "o4")
	{   
		Coefficient = lr_wheels_distance_ / 2.0 + fr_wheels_distance_ / 2.0;
		encoder_data_x_  = (-motor_encoder1_inc + motor_encoder2_inc - \
							 motor_encoder3_inc + motor_encoder4_inc)/4.0;
		encoder_data_y_  = (motor_encoder1_inc - motor_encoder2_inc + \
							 motor_encoder3_inc - motor_encoder4_inc)/4.0;
		encoder_angle_z_ = ((motor_encoder1_inc + motor_encoder2_inc + \
							 motor_encoder3_inc + motor_encoder4_inc)/4.0)/Coefficient;	
	}
	else if(baseType == "o3")
	{
	
		Coefficient = lr_wheels_distance_ / 2.0 + fr_wheels_distance_ / 2.0;

		encoder_data_x_  = (-motor_encoder1_inc + motor_encoder2_inc - \
							 motor_encoder3_inc + motor_encoder4_inc)/4.0;

		encoder_data_y_  = (motor_encoder1_inc - motor_encoder2_inc + \
							 motor_encoder3_inc - motor_encoder4_inc)/4.0;

		encoder_angle_z_ = ((motor_encoder1_inc + motor_encoder2_inc + \
							 motor_encoder3_inc + motor_encoder4_inc)/4.0)/Coefficient;		
	}
	else
	{   
		Coefficient = lr_wheels_distance_ / 2.0 + fr_wheels_distance_ / 2.0;
		encoder_data_x_  = (motor_encoder1_inc + motor_encoder2_inc + \
							motor_encoder3_inc + motor_encoder4_inc)/4.0;
		encoder_data_y_  = (motor_encoder2_inc + motor_encoder4_inc - \
							motor_encoder1_inc - motor_encoder3_inc)/4.0;
		encoder_angle_z_ = ((motor_encoder2_inc + motor_encoder4_inc - \
							motor_encoder1_inc - motor_encoder3_inc)/4.0)/Coefficient;
		encoder_data_x_  = encoder_data_x_ * linear_scale_correction_;
		encoder_angle_z_ = encoder_angle_z_* angular_scale_wheel_;		
	}
	x_pos_ += (encoder_data_x_ * cos(heading_) - encoder_data_y_ * sin(heading_));
	y_pos_ += (encoder_data_x_ * sin(heading_) + encoder_data_y_ * cos(heading_));
	heading_ += encoder_angle_z_;

	//calculate robot's heading in quaternion angle
	//ROS has a function to calculate yaw in quaternion angle
	odom_quat = tf::createQuaternionMsgFromYaw(heading_);
	//odom_quat.setRPY(0,0,heading_);
	odom_trans.header.stamp = current_time;
	odom_trans.header.frame_id = tf_frame_id_name_;
	odom_trans.child_frame_id = tf_child_frame_id_name_;
	//robot's position in x,y, and z
	odom_trans.transform.translation.x = x_pos_;
	odom_trans.transform.translation.y = y_pos_;
	odom_trans.transform.translation.z = 0.0;
	//robot's heading in quaternion
	odom_trans.transform.rotation = odom_quat;
	//publish robot's tf using odom_trans object
	if(pub_encoder_odom_tf_)
	{
		odom_broadcaster_.sendTransform(odom_trans);
	  }
	odom.header.stamp = current_time;
	odom.header.frame_id = odom_frame_id_name_;
	odom.child_frame_id = tf_child_frame_id_name_;

	//robot's position in x,y, and z
	odom.pose.pose.position.x = x_pos_;
	odom.pose.pose.position.y = y_pos_;
	odom.pose.pose.position.z = 0.0;
	//robot's heading in quaternion
	odom.pose.pose.orientation = odom_quat;
	odom.pose.covariance[0] = 0.001;
	odom.pose.covariance[7] = 0.001;
	odom.pose.covariance[35] = 0.001;

	//linear speed from encoders
	odom.twist.twist.linear.x = linear_velocity_x_;
	odom.twist.twist.linear.y = linear_velocity_y_;
	odom.twist.twist.linear.z = 0.0;

	odom.twist.twist.angular.x = 0.0;
	odom.twist.twist.angular.y = 0.0;
	//angular speed from encoders
	odom.twist.twist.angular.z = angular_velocity_z_;
	odom.twist.covariance[0] = 0.0001;
	odom.twist.covariance[7] = 0.0001;
	odom.twist.covariance[35] = 0.0001;

	odom_publisher_.publish(odom);
	if(is_sub_node_)
	{
		ROS_INFO("Subscribe wheel node data successful");
		is_sub_node_ = false;
	}	
	last_vel_time_ = current_time;
	motor_encoder1_old = vel.encoder_motor1;
	motor_encoder2_old = vel.encoder_motor2;
	motor_encoder3_old = vel.encoder_motor3;
	motor_encoder4_old = vel.encoder_motor4;
}
